/*
 * @Description: 运动控制线程
 * @Author: Dryad
 * @Date: 2019-07-03 10:01:06
 */

#include <rtthread.h>
#include <rthw.h>
#include <math.h>
#include "./Move_Control.h"
#include "../Drivers/Mecanum_AGV.hpp"
#include "./Coordinate_Velocity.hpp"
#include "./Location.h"
#include "../Math/My_Math.hpp"
#include "../parameter.h"
#include "../Math/Bezier.hpp"
#include "./Interpolation.hpp"
#include "../Math/PID_Control.hpp"
#include "./Avoid.h"

#define LOG_TAG "move control" /* 该模块TAG为move control */
#define LOG_LVL LOG_LVL_DBG    /* 静态过滤级别为调试级 */
#include <ulog.h>              /* 必须放在宏定义下面 */

static Bezier_Curve_Class *first_bezier_curve = nullptr;    /* 指向第一条曲线 */
static PID_Control_Class vx_pid, vy_pid, w_pid;             /* 速度的PID控制器 */
static enum AGV_Move_State_Enum agv_move_state = Move_IDLE; /* 默认为运动空闲 */
static Interpolation_Class interpolation;                   /* 插补 */

static Coordinate_Class virtual_coor_inworld, current_coor_inworld;       /* 虚拟坐标和真实坐标 */
static Velocity_Class virtual_velocity_inworld, control_velocity_inagv;   /* 虚拟速度和控制速度 */
static union Bezier_Curve_Class::Bezier_Control_Coor virtual_bezier_coor; /* 虚拟Bezier曲线坐标 */

static rt_event move_control_event;           /* 运动控制事件集,事件见Move_Control.h文件枚举定义 */
static struct rt_messagequeue bezier_message; /* 接收一条Bezier曲线的数据消息队列 */
ALIGN(RT_ALIGN_SIZE)
char rx_data_message_pool[(sizeof(struct Interpolation_Class::Interpolation_Parameter_Struct) + 4) * 16]; /* 消息队列内存，数据大小+消息头指针，最多16个消息 */

static struct rt_thread bezier_control_thread;
static void Bezier_Control_Thread(void *parameter); /* 运动控制函数 */
ALIGN(RT_ALIGN_SIZE)                                /* 线程栈4字节对齐 */
static char bezier_control_thread_stack[4096];      /* 线程栈起始地址 */

static void Up_Velocity_By_ErrorCoor(const Coordinate_Class &agv_current_coor_inworld,
                                     const Coordinate_Class &agv_target_coor_inworld,
                                     Velocity_Class &agv_target_velocity_inagv); /* 根据跟踪误差更新控制速度 */
static void Update_Move_State(void);                                             /* 根据暂停、取消、继续等事件更新运动状态 */

static void Trans_Bezier_Coor(Coordinate_Class &coor,
                              union Bezier_Curve_Class::Bezier_Control_Coor &bezier_coor, bool to_bezier_flag); /* 转换普通坐标和Bezier曲线坐标 */
static void Trans_Bezier_Coor(Velocity_Class &coor,
                              union Bezier_Curve_Class::Bezier_Control_Coor &bezier_coor, bool to_bezier_flag); /* 转换普通坐标和Bezier曲线坐标 */
static void Free_Bezier(Bezier_Curve_Class *origin);                                                            /* 释放Bezier曲线占用的内存 */
static float Cal_V_ABS(const Velocity_Class &coor);                                                             /* 计算速度的一范数 */

static bool Check_Brake_Event(void);                                           /* 检查停车事件 */
static void Update_Velocity_By_Sensor(Velocity_Class &control_velocity_inagv); /* 根据避障信息更新控制速度，并更新上一周期的控制速度 */

static float pre_v_abs, current_v_abs; /* 上一时刻的速度值，和当前时刻速度值 */
static float v_acc, v_dec;             /* 速度的增加减少值 */
static float dec_emergency_stop;       /* 急停时每个控制周期的减速度 */
static enum Sensor_Enum current_sensor_state = Sensor_Normal;

static struct rt_semaphore reset_sem;

static float Update_Velocity(const struct Velocity_Inc_Dec_Struct &velocity, const float min_velocity_abs); /* 速度加减速处理 */
struct Velocity_Inc_Dec_Struct
{
    float target_velocity; /* 目标速度 */
    float pre_velocity;    /* 上一时刻的速度 */
    float increase;        /* 速度的增加值 */
    float decrease;        /* 速度的降低值 */
} target_velocity_temp;    /* 用于速度的加减速结构体 */

void Move_Control_Thread_Init(void)
{
    rt_mq_init(&bezier_message, "bezier control message", rx_data_message_pool,
               sizeof(struct Interpolation_Class::Interpolation_Parameter_Struct), sizeof(rx_data_message_pool), RT_IPC_FLAG_FIFO); /* 初始化消息队列 */
    rt_event_init(&move_control_event, "move control event", RT_IPC_FLAG_FIFO);                                                     /* 运动控制事件集 */

    rt_sem_init(&reset_sem, "angle reset sem", 0, RT_IPC_FLAG_FIFO);

    Mecanum_AGV_Class::Init(agv_parameter->distance_x, agv_parameter->distance_y,
                            agv_parameter->max_velocity_line, agv_parameter->motor_max_rpm); /* 初始化电机 */
    first_bezier_curve = nullptr;

    vx_pid.Init(2.0f, 0.0f, 0.0f);
    vy_pid.Init(2.0f, 0.0f, 0.0f);
    w_pid.Init(2.0f, 0.0f, 0.0f);
    vx_pid.Set_Limit(0.1f * agv_parameter->acc_vx, 0.1f * agv_parameter->acc_vx);
    vy_pid.Set_Limit(0.1f * agv_parameter->acc_vy, 0.1f * agv_parameter->acc_vy);
    w_pid.Set_Limit(0.1f * agv_parameter->acc_w, 0.1f * agv_parameter->acc_w);

    agv_move_state = Move_IDLE;
    interpolation.interpolation_state = Interpolation_Class::NO_Interpolation;
    virtual_coor_inworld.Clear();
    current_coor_inworld.Clear();
    virtual_velocity_inworld.Clear();
    control_velocity_inagv.Clear();

    pre_v_abs = 0.0f;
    current_v_abs = 0.0f;
    v_acc = 0.0f;
    v_dec = 0.0f;
    dec_emergency_stop = agv_parameter->max_velocity_line / agv_parameter->dec_emergency_time * 0.01f; /* 触发急停时的减速度 */

    rt_err_t rt_result;

    rt_result = rt_thread_init(&bezier_control_thread, "beziern control thread", Bezier_Control_Thread, RT_NULL, bezier_control_thread_stack, sizeof(bezier_control_thread_stack), 7, 5);
    if (rt_result == RT_EOK)
    {
        rt_thread_startup(&bezier_control_thread); /* 启动线程 */
    }
    else
    {
        rt_kprintf("bezier control thread error\n");
    }
}

enum AGV_Move_State_Enum Read_AGV_Move_State(void)
{
    return agv_move_state;
}

rt_err_t Set_Destination(const void *destination)
{
    rt_err_t result;
    result = rt_mq_send(&bezier_message, destination, sizeof(struct Interpolation_Class::Interpolation_Parameter_Struct));
    return result;
}

short Return_Avoid_Check(void)
{
    return (short)current_sensor_state;
}

void Write_Brake_Event(enum Motor_Brake_Event_Enum value)
{
    rt_event_send(&move_control_event, 1 << (int)value); /* 发送事件 */
}

void Write_Move_Control_Event(enum Move_Control_Event_Enum value)
{
    rt_event_send(&move_control_event, 1 << (int)value); /* 发送事件 */
}

/* Bezier曲线控制 */
void Bezier_Control_Thread(void *parameter)
{
    LOG_I("bezier control start."); /* 位置控制模式 */

    rt_err_t rt_result;

    while (1)
    {
        rt_result = rt_mq_recv(&bezier_message, &interpolation.interpolation_parameter,
                               sizeof(struct Interpolation_Class::Interpolation_Parameter_Struct), 500); /* 等待接收新指令 */

        if (-RT_ETIMEOUT == rt_result)
        {
            current_sensor_state = Sensor_Normal;     /* 当前避障状态 */
            agv_move_state = Move_IDLE;               /* 运动空闲 */
            Mecanum_AGV_Class::Write_Zero_Velocity(); /* 零速 */
            continue;
        }

        first_bezier_curve = (Bezier_Curve_Class *)interpolation.interpolation_parameter.user_data; /* 保存第一段曲线 */

        if (RT_EOK == rt_sem_take(&reset_sem, 0))
        {
            /* 重置角度数据 */
            virtual_coor_inworld.Angle_Trans(0.0f); /* 重置当前虚拟角度 */
            rt_sem_control(&reset_sem, RT_IPC_CMD_RESET, 0);
        }

        Check_Brake_Event();

        Trans_Bezier_Coor(virtual_coor_inworld, virtual_bezier_coor, true);
        LOG_I("origin:%.2f %.2f %.2f.", virtual_coor_inworld.x_coor, virtual_coor_inworld.y_coor, virtual_coor_inworld.angle_coor);

        /* 生成Bezier曲线，并计算长度 */
        float sum_length = 0.0f;
        Bezier_Curve_Class *current_temp = first_bezier_curve;

        union Bezier_Curve_Class::Bezier_Control_Coor origin_bezier_coor;
        rt_memcpy(&origin_bezier_coor, &virtual_bezier_coor, sizeof(union Bezier_Curve_Class::Bezier_Control_Coor));

        {
            for (const union Bezier_Curve_Class::Bezier_Control_Coor *coor_temp = &origin_bezier_coor; current_temp != nullptr; current_temp = (Bezier_Curve_Class *)(current_temp->next_curve))
            {
                current_temp->P4.z = My_Math_Class::Trans_Rad(current_temp->P4.z);
                current_temp->P4.z *= agv_parameter->distance_lx_ly;
                sum_length += current_temp->Generate_Curve(coor_temp);
                coor_temp = &current_temp->P4;
            }
        }

        bool result = interpolation.Init(sum_length); /* 插补速度 */

        /* 根据插补结果判断是否执行该指令 */
        if (!result)
        {
            /* 移动距离小于阈值 */
            Mecanum_AGV_Class::Write_Zero_Velocity(); /* 零速 */
            Free_Bezier(first_bezier_curve);          /* 释放内存空间 */
            agv_move_state = Move_Over;               /* 当前运动指令结束 */
            LOG_I("threshold small.");
            continue; /* 等待下一个指令 */
        }
        else
        {
            agv_move_state = Move_Normal; /* 车辆正常运行 */
            v_acc = interpolation.interpolation_parameter.acc * 0.01f;
            v_dec = interpolation.interpolation_parameter.dec * 0.01f;
        }

        /* 清空暂停取消等事件 */
        {
            rt_uint32_t e = 0;
            rt_event_recv(&move_control_event, 1 << Move_Control_Continue | 1 << Move_Control_Pause | 1 << Move_Control_Cancel,
                          RT_EVENT_FLAG_CLEAR | RT_EVENT_FLAG_OR, RT_WAITING_NO, &e);
        }
        rt_thread_delay(500);                               /* 延时500ms */
        LOG_I("start move,path length: %.2f.", sum_length); /* 开始路径跟踪 */

        float v_abs = 0.0f;
        current_temp = first_bezier_curve;
        sum_length = 0.0f;
        Bezier_Curve_Class::curve_length = 0.0f; /* 当前曲线参数u对应的曲线长度 */
        Bezier_Curve_Class::curve_u = 0.0f;      /* 曲线参数u */

        while (1)
        {
            if (Check_Brake_Event())
            {
                Free_Bezier(first_bezier_curve); /* 释放运动块 */
                break;
            };

            /* 获取当前坐标 */
            Read_Location_Coor((struct Location_Coor_Struct *)&current_coor_inworld.coor[0]); /* 强制类型转换，数据本身内存分布是一样的 */
            // current_coor_inworld.Angle_Trans(virtual_coor_inworld.angle_coor);
            Trans_Bezier_Coor(current_coor_inworld, virtual_bezier_coor, true);

            // LOG_D("当前坐标:%.2f,%.2f,%.2f", current_coor_inworld.x_coor, current_coor_inworld.y_coor, current_coor_inworld.angle_coor);
            // LOG_D("Bezier坐标:%.2f,%.2f,%.2f", virtual_bezier_coor.x, virtual_bezier_coor.y, virtual_bezier_coor.z);

            // float u_temp = Bezier_Curve_Class::curve_u;

            result = current_temp->Cal_Target(virtual_bezier_coor); /* 计算目标坐标和目标速度 */

            v_abs = interpolation.Cal_Velocity(sum_length + Bezier_Curve_Class::curve_length); /* 计算速度大小 */

            virtual_bezier_coor = Bezier_Curve_Class::target_coor; /* 获取目标坐标 */
            Trans_Bezier_Coor(virtual_coor_inworld, virtual_bezier_coor, false);
            virtual_bezier_coor = Bezier_Curve_Class::target_v; /* 获取速度方向 */
            Trans_Bezier_Coor(virtual_velocity_inworld, virtual_bezier_coor, false);

            /* 根据速度方向计算AGV速度 */
            Velocity_Class::Absolute_To_Relative(virtual_velocity_inworld, control_velocity_inagv, virtual_coor_inworld); /* 转换至AGV坐标系 */
            float v_abs_inagv = Cal_V_ABS(control_velocity_inagv);
            // LOG_D("u:%.2f,v %.2f , %.2f", Bezier_Curve_Class::curve_u, v_abs, v_abs_inagv);

            if ((v_abs_inagv < agv_parameter->min_velocity_line) && (FP_ZERO != fpclassify(v_abs_inagv)))
            {
                /* 将电机无法响应的低速放大 */
                control_velocity_inagv *= (agv_parameter->min_velocity_line / v_abs_inagv);
            }
            else
            {
                control_velocity_inagv *= (v_abs / v_abs_inagv);
            }

            Up_Velocity_By_ErrorCoor(current_coor_inworld, virtual_coor_inworld, control_velocity_inagv); /* 根据跟踪误差更新控制速度 */
            Update_Move_State();                                                                          /* 根据运动控制事件更新速度 */
            Update_Velocity_By_Sensor(control_velocity_inagv);                                            /* 根据避障传感器更新控制速度 */

            if ((Move_Cancel == agv_move_state) && (FP_ZERO == fpclassify(pre_v_abs)))
            {
                /* 当前为取消任务，且速度已经为零 */
                Free_Bezier(first_bezier_curve);          /* 释放内存空间 */
                agv_move_state = Move_IDLE;               /* 运动空闲 */
                Mecanum_AGV_Class::Write_Zero_Velocity(); /* 设置电机零速 */
                rt_thread_delay(100);                     /* 延时一定时间 */
                break;
            }

            if (!result)
            {
                /* 该段曲线已运动完毕 */
                if (current_temp->next_curve)
                {
                    /* 下一段曲线 */
                    sum_length += current_temp->sum_length;                        /* 更新已运动过得曲线长度 */
                    current_temp = (Bezier_Curve_Class *)current_temp->next_curve; /* 切换到下一段曲线 */
                    Bezier_Curve_Class::curve_u = 0.0f;                            /* 曲线参数u清零 */
                    agv_move_state = Move_Over;                                    /* 当前曲线运动结束 */
                    Mecanum_AGV_Class::Write_Velocity(control_velocity_inagv);     /* 控制AGV运动 */
                    // LOG_D("control v:%.2f,%.2f,%.2f", control_velocity_inagv.velocity_x, control_velocity_inagv.velocity_y, control_velocity_inagv.velocity_angle);

                    continue;
                }
                else
                {
                    /* 不存在下一条曲线 */
                    LOG_I("move over."); /* 运动结束 */

                    Mecanum_AGV_Class::Write_Zero_Velocity(); /* 设置电机零速 */

                    rt_memcpy(&virtual_bezier_coor, &current_temp->P4, sizeof(union Bezier_Curve_Class::Bezier_Control_Coor)); /* 保存虚拟坐标 */
                    Trans_Bezier_Coor(virtual_coor_inworld, virtual_bezier_coor, false);                                       /* 计算虚拟坐标 */

                    Free_Bezier(first_bezier_curve); /* 释放内存空间 */
                    agv_move_state = Move_IDLE;      /* 运动空闲 */
                    break;
                }
            }

            Mecanum_AGV_Class::Write_Velocity(control_velocity_inagv); /* 控制AGV运动 */
        }
    }
}

/* 根据跟踪误差更新控制速度 */
void Up_Velocity_By_ErrorCoor(const Coordinate_Class &agv_current_coor_inworld, /* 当前坐标 */
                              const Coordinate_Class &agv_target_coor_inworld,  /* 实际坐标 */
                              Velocity_Class &control_velocity_inagv            /* 控制速度 */
)
{
    Coordinate_Class tracking_error;
    tracking_error = agv_target_coor_inworld - agv_current_coor_inworld; /* 计算跟踪误差 */
    float vx_delta, vy_delta, w_delta;
    // LOG_D("tarck error:%.2f %.2f %.2f", tracking_error.x_coor, tracking_error.y_coor, tracking_error.angle_coor);
    // LOG_D("control v:%.2f %.2f %.2f", control_velocity_inagv.velocity_x, control_velocity_inagv.velocity_y, control_velocity_inagv.velocity_angle);

    // if (My_Math_Class::ABS(control_velocity_inagv.velocity_x) > 20.0f)
    // {
    //     asm("nop");
    // }

    vx_delta = vx_pid.Cal(tracking_error.x_coor);
    vy_delta = vy_pid.Cal(tracking_error.y_coor);
    w_delta = w_pid.Cal(tracking_error.angle_coor);
    // LOG_D("delta v:%.2f,%.2f,%.2f", vx_delta, vy_delta, w_delta);
    control_velocity_inagv.velocity_x += vx_delta;    /* 更新横向速度 */
    control_velocity_inagv.velocity_y += vy_delta;    /* 更新纵向速度 */
    control_velocity_inagv.velocity_angle += w_delta; /* 更新角速度 */
}

void Update_Move_State(void)
{
    rt_uint32_t event_value = 0;
    rt_event_recv(&move_control_event, (1 << Move_Control_Continue) | (1 << Move_Control_Pause) | (1 << Move_Control_Cancel),
                  RT_EVENT_FLAG_CLEAR | RT_EVENT_FLAG_OR, RT_WAITING_NO, &event_value); /* 接收任务暂停、取消、继续事件 */

    if (event_value & (1 << Move_Control_Cancel))
    {
        /* 接收到取消任务事件 */
        agv_move_state = Move_Cancel; /* AGV状态为取消任务中 */
        LOG_I("task cancel.");        /* 任务取消 */
    }
    else if (event_value & (1 << Move_Control_Pause))
    {
        /* 接收到暂停任务事件 */
        agv_move_state = Move_Stop_For_Waitting; /* AGV状态为停车等待 */
        LOG_I("task pause.");                    /* 任务暂停 */
    }
    else if ((event_value & (1 << Move_Control_Continue)) && (Move_Stop_For_Waitting == agv_move_state))
    {
        /* 接收到任务继续事件且当前为停车等待 */
        agv_move_state = Move_Normal; /* AGV状态为正常运行 */
        LOG_I("task continum.");      /* 任务继续 */
    }
}

void Trans_Bezier_Coor(Coordinate_Class &coor,
                       union Bezier_Curve_Class::Bezier_Control_Coor &bezier_coor, bool to_bezier_flag)
{
    if (to_bezier_flag)
    {
        bezier_coor.x = coor.x_coor;
        bezier_coor.y = coor.y_coor;
        bezier_coor.z = My_Math_Class::Trans_Rad(coor.angle_coor);
        bezier_coor.z *= agv_parameter->distance_lx_ly;
    }
    else
    {
        coor.x_coor = bezier_coor.x;
        coor.y_coor = bezier_coor.y;
        coor.angle_coor = My_Math_Class::Trans_Angle(bezier_coor.z);
        coor.angle_coor /= agv_parameter->distance_lx_ly;
    }
}

void Trans_Bezier_Coor(Velocity_Class &coor,
                       union Bezier_Curve_Class::Bezier_Control_Coor &bezier_coor, bool to_bezier_flag)
{
    if (to_bezier_flag)
    {
        bezier_coor.x = coor.velocity_x;
        bezier_coor.y = coor.velocity_y;
        bezier_coor.z = coor.velocity_angle;
        bezier_coor.z = bezier_coor.z / 180.0f * PI;
        bezier_coor.z *= agv_parameter->distance_lx_ly;
    }
    else
    {
        coor.velocity_x = bezier_coor.x;
        coor.velocity_y = bezier_coor.y;
        coor.velocity_angle = bezier_coor.z;
        coor.velocity_angle /= agv_parameter->distance_lx_ly;
        coor.velocity_angle = coor.velocity_angle / PI * 180.0f;
    }
}

void Free_Bezier(Bezier_Curve_Class *origin)
{
    Bezier_Curve_Class *temp = origin;
    if (temp)
    {
        while (temp->next_curve)
        {
            temp = (Bezier_Curve_Class *)temp->next_curve;
            rt_free((Bezier_Curve_Class *)temp->pre_curve);
        }
        rt_free((Bezier_Curve_Class *)temp);
    }
}

float Cal_V_ABS(const Velocity_Class &coor)
{
    float abs_temp;
    abs_temp = My_Math_Class::ABS(coor.velocity_x) + My_Math_Class::ABS(coor.velocity_y);
    float temp = coor.Trans_Rad();
    temp *= agv_parameter->distance_lx_ly;
    abs_temp += My_Math_Class::ABS(temp);
    return abs_temp;
}

bool Check_Brake_Event(void)
{
    /* 若第一次读取到二维码，则更新起始坐标 */
    rt_uint32_t e = 0;
    rt_err_t rt_result = rt_event_recv(&move_control_event, (1 << First_Read_DM) | (1 << Communication_Timeout),
                                       RT_EVENT_FLAG_CLEAR | RT_EVENT_FLAG_OR, 10, &e); /* 控制周期大约为10ms */
    if (RT_EOK == rt_result)
    {
        /* 读取到了停车事件,AGV需刹车停下 */
        agv_move_state = Move_IDLE;               /* 任务状态为空闲 */
        current_sensor_state = Sensor_Normal;     /* 避障状态正常 */
        Mecanum_AGV_Class::Write_Zero_Velocity(); /* 设置电机零速 */

        if (e & ((1 << First_Read_DM)))
        {
            /* 第一次读取到二维码,则更新虚拟坐标 */
            Read_Location_Coor((struct Location_Coor_Struct *)&virtual_coor_inworld.coor[0]); /* 强制类型转换，数据本身内存分布是一样的 */
            LOG_I("first read DM:%.2f,%.2f,%.2f", virtual_coor_inworld.x_coor, virtual_coor_inworld.y_coor, virtual_coor_inworld.angle_coor);
        }
        else
        {
            LOG_W("time out.");
        }
        return true;
    }
    return false;
}

void Update_Velocity_By_Sensor(Velocity_Class &control_velocity_inagv)
{
    /* 计算当前速度大小 */
    current_v_abs = My_Math_Class::ABS(control_velocity_inagv.velocity_x) + My_Math_Class::ABS(control_velocity_inagv.velocity_y);
    current_v_abs += My_Math_Class::ABS(control_velocity_inagv.Trans_Rad() * agv_parameter->distance_lx_ly);

    /* 计算线速度方向，判断当前使用的避障传感器 */
    float current_velocity_angle = atan2f(-control_velocity_inagv.velocity_x, control_velocity_inagv.velocity_y); /* 计算速度方向与y轴夹角(rad) */
    current_velocity_angle = My_Math_Class::Trans_Angle(current_velocity_angle);                                  /* 转换为° */

    /* 根据线速度方向判断使用的传感器 */
    enum Sensor_Enum sensor_state = Sensor_Normal; /* 默认避障无触发 */
    int avoid_index = -1;                          /* 默认不使用避障传感器 */
    {
        float angle_temp[4]; /* 判断避障传感器对应的4个角度 */
        angle_temp[0] = agv_parameter->agv_overall_offset_angle - 180.0f;
        angle_temp[1] = -agv_parameter->agv_overall_offset_angle;
        angle_temp[2] = -angle_temp[1];
        angle_temp[3] = -angle_temp[0];
        if ((current_velocity_angle < angle_temp[1]) && (current_velocity_angle >= angle_temp[0]))
        {
            avoid_index = 3; /* 使用右避障 */
        }
        else if ((current_velocity_angle < angle_temp[2]) && (current_velocity_angle >= angle_temp[1]))
        {
            avoid_index = 0; /* 使用前避障 */
        }
        else if ((current_velocity_angle < angle_temp[3]) && (current_velocity_angle >= angle_temp[2]))
        {
            avoid_index = 2; /* 使用左避障 */
        }
        else
        {
            avoid_index = 1; /* 使用后避障 */
        }
    }

    if (avoid_index != -1)
    {
        sensor_state = Avoid_Read(avoid_index); /* 读取避障传感器 */
    }

    /* 读取前后避障信息，用于判断是否触发急停 */
    enum Sensor_Enum front_brake, behind_brake;
    front_brake = Avoid_Read(0);
    behind_brake = Avoid_Read(1);
    unsigned short emergency[2];
    emergency[0] = Read_Emergency(0);
    emergency[1] = Read_Emergency(1);

    if (emergency[0] || emergency[1])
    {
        LOG_I("emergency stop.");
        sensor_state = Sensor_Brake;
        Write_Move_Control_Event(Move_Control_Cancel); /* 任务取消 */
    }
    else if ((Sensor_Brake == front_brake) || (Sensor_Brake == behind_brake))
    {
        /* 触发防撞条 */
        LOG_I("sensor brake trigger.");
        sensor_state = Sensor_Brake;
    }

    bool move_norm_flag = ((Move_Cancel != agv_move_state) && (Move_Stop_For_Waitting != agv_move_state)); /* true表示不为取消任务也不为暂停任务 */

    float current_v_k;                   /* 当前速度的比例系数 */
    float target_v;                      /* 目标速度 */
    current_sensor_state = sensor_state; /* 保存当前避障传感器状态 */
    if (Sensor_Brake == sensor_state)
    {
        /* 触发防撞条 */
        Mecanum_AGV_Class::Write_Zero_Velocity();                    /* 写入零速 */
        Mecanum_AGV_Class::Set_Motor_Mode(Motor_Class::Motor_Brake); /* 电机急停 */
        current_v_abs = 0.0f;
        LOG_I("sensor brake."); /* 触发避障 */
        if (move_norm_flag)
        {
            /* 当期不为取消任务或者暂停任务 */
            agv_move_state = Move_Trigger_Sensor; /* 车辆正常运行 */
        }

        {
            rt_err_t rt_result;
            rt_uint32_t event_value = 0;
            for (int i = 0; i < 100; i++)
            {
                /* 延时10s,100ms检查一次 */
                rt_result = rt_event_recv(&move_control_event, 1 << Move_Control_Continue | 1 << Move_Control_Pause | 1 << Move_Control_Cancel,
                                          RT_EVENT_FLAG_CLEAR | RT_EVENT_FLAG_OR, 100, &event_value); /* 接收任务暂停、取消、继续事件 */
                if (RT_EOK == rt_result)                                                              /* 读取到了任务取消事件 */
                {
                    if (event_value & (1 << Move_Control_Cancel))
                    {
                        /* 接收到了任务取消事件 */
                        agv_move_state = Move_Cancel; /* 任务取消 */
                    }
                    else if (event_value & (1 << Move_Control_Pause))
                    {
                        /* 接收到了任务暂停事件 */
                        agv_move_state = Move_Stop_For_Waitting; /* 任务暂停 */
                    }
                    break;
                }
                else if ((-RT_ETIMEOUT == rt_result))
                {
                    /* 若100ms后发现仍为抱闸，重新计数 */
                    front_brake = Avoid_Read(0);
                    behind_brake = Avoid_Read(1);
                    emergency[0] = Read_Emergency(0);
                    emergency[1] = Read_Emergency(1);

                    if (emergency[0] || emergency[1])
                    {
                        i = 50;
                    }
                    else if ((Sensor_Brake == front_brake) || (Sensor_Brake == behind_brake))
                    {
                        i = 0;
                    }
                }
            }
            LOG_I("brake over.");                                         /* 急停解除 */
            Mecanum_AGV_Class::Set_Motor_Mode(Motor_Class::Motor_Enable); /* 电机使能 */
            rt_thread_delay(500);                                         /* 延时500ms */
        }
    }
    else if (!move_norm_flag)
    {
        /* 当前为取消或者暂停任务 */
        target_velocity_temp.target_velocity = 0.0f;
        target_velocity_temp.pre_velocity = pre_v_abs;
        target_velocity_temp.increase = v_acc;
        target_velocity_temp.decrease = (Sensor_Stop == sensor_state) ? dec_emergency_stop : v_dec;
        target_v = Update_Velocity(target_velocity_temp, 0.0f);

        if ((My_Math_Class::ABS(target_v) < agv_parameter->min_velocity_line))
        {
            current_v_k = 0.0f;
        }
        else
        {
            current_v_k = My_Math_Class::ABS(target_v / current_v_abs);
        }
    }
    else
    {
        switch (sensor_state)
        {
        case Sensor_Normal:
        case Sensor_Alarm:
        {
            agv_move_state = Move_Normal; /* 正常运行 */
            // current_v_k = 1.0f;

            target_velocity_temp.target_velocity = current_v_abs;
            target_velocity_temp.pre_velocity = pre_v_abs;
            target_velocity_temp.increase = v_acc;
            target_velocity_temp.decrease = v_dec;
            target_v = Update_Velocity(target_velocity_temp, agv_parameter->min_velocity_line);
            current_v_k = My_Math_Class::ABS(target_v / current_v_abs);
        }
        break;
        case Sensor_Slow:
        case Sensor_Stop:
        {
            agv_move_state = Move_Trigger_Sensor; /* 避障触发 */
            float min_velocity_line_temp;
            if (Sensor_Stop == sensor_state)
            {
                min_velocity_line_temp = 0.0f;
            }
            else
            {
                min_velocity_line_temp = agv_parameter->obstacle_velocity;
            }

            target_velocity_temp.target_velocity = 0.0f;
            target_velocity_temp.pre_velocity = pre_v_abs;
            target_velocity_temp.increase = target_velocity_temp.increase;
            target_velocity_temp.decrease = (Sensor_Stop == sensor_state) ? dec_emergency_stop : v_dec;
            target_v = Update_Velocity(target_velocity_temp, min_velocity_line_temp);

            if ((My_Math_Class::ABS(target_v) < agv_parameter->min_velocity_line))
            {
                current_v_k = 0.0f;
            }
            else
            {
                current_v_k = My_Math_Class::ABS(target_v / current_v_abs);
            }
        }
        break;
        default:
            break;
        }
    }

    /*更新速度控制量*/
    pre_v_abs = current_v_abs * current_v_k;
    control_velocity_inagv *= current_v_k;
}

float Update_Velocity(const struct Velocity_Inc_Dec_Struct &velocity, const float min_velocity_abs)
{
    float target_velocity;
    target_velocity = My_Math_Class::Range(velocity.target_velocity, velocity.pre_velocity - velocity.decrease, velocity.pre_velocity + velocity.increase);
    if (target_velocity < 0.0f)
    {
        target_velocity = My_Math_Class::Min(target_velocity, -min_velocity_abs);
    }
    else
    {
        target_velocity = My_Math_Class::Max(target_velocity, min_velocity_abs);
    }
    return target_velocity;
}

void Reset_Virtual_Angle(void)
{
    rt_sem_release(&reset_sem);
}
